#include "Controller.h"

using namespace std;

Controller::Controller(): nh_ ("~"), limit_(2), DOOR(0.3)
{
  std::string scan_topic;
  std::string cmd_vel_topic,odom_topic;
  std::string openGLDebug="/openGLDebug";
  std::string circle_topic="/target_pos";
  
  // threshold to clean scans
  // Reads params from file
  nh_.param<std::string>("scan_topic", scan_topic, "/scan");
  nh_.param<std::string>("cmd_vel_topic", cmd_vel_topic, "/cmd_vel");
  nh_.param<std::string>("odom_topic", odom_topic, "/pose");
  nh_.param<bool>("flipped",flipped,false);

  ROS_INFO("Subscribing to topic %s",scan_topic.c_str()); 
  scan_sub_ = nh_.subscribe (scan_topic, 1, &Controller::newLaserScan, this);
  circle_sub_ = nh_.subscribe(circle_topic, 1, &Controller::newBallCenter, this);
  odom_sub_= nh_.subscribe(odom_topic,1,&Controller::newOdometry, this);
  cmd.linear.x=0;
  cmd.linear.y=0;
  cmd.linear.z=0;
  cmd.angular.x=0;
  cmd.angular.y=0;
  cmd.angular.z=0;
  ROS_INFO("Advertising topic %s",cmd_vel_topic.c_str()); 
  vel_pub_ = nh_.advertise<geometry_msgs::Twist>(cmd_vel_topic, 1);
  openglPub= nh_.advertise<BonzoSurvivors::Beams>(openGLDebug,1);
  ROS_INFO("flipped = %d",flipped);
  client_= nh_.serviceClient<gripper_driver::Messaggio>("/gripperdriver/messaggio");
  
  defaultLinear_=0.4f;
  defaultTurn_=0.4f;
  
  nearDistance_=1.0;
  centerTollerance_=0.015;
  
  ballCenter_.x=NAN;
  ballCenter_.y=NAN;
  ballCenter_.z=NAN;
  pallaAlzata_=false;
  targetFound_=false;
  targetDetectionActivate_=true;
  obstacleAvoidance_=true;
  status_=WANDERING;
  linear_=defaultLinear_;
  accuFront_=0;
  accuNear_=0;
  
}

Controller::Controller(const Controller& orig) {
}

Controller::~Controller() {
}

double Controller::histogramOA(float aMin, float aMax)
{
  std::pair<double,double> interval(-1,-1);
  
  double min=-1,sumSx=0,sumDx=0,curve=1.1;
  double minBeam=50,alphaMin;
  for (float a = aMin; a < aMax; a+=scan_.angle_increment) {
    if (scan_.ranges[aToRange(a)]>scan_.range_min && scan_.ranges[aToRange(a)]<scan_.range_max) {
      if(scan_.ranges[aToRange(a)]>=limit_*(1+abs(a)) && min==-1)
        min=a;
      else if (scan_.ranges[aToRange(a)]<limit_*(1+abs(a)) && min!=-1){
        if(abs(min-a)>abs(interval.first-interval.second))
          interval=std::pair<double,double>(min,a);
        min=-1;
      }
      if(scan_.ranges[aToRange(a)]<minBeam){
          minBeam=scan_.ranges[aToRange(a)];
          alphaMin=a;
      }
    }
  }
  if(min!=-1)
    interval=std::pair<double,double>(min,M_PI/2);
  //std::cout<<"min a: "<<alphaMin*180/M_PI<<std::endl;
  for (float a = aMin; a < aMax; a+=scan_.angle_increment) {
     if (scan_.ranges[aToRange(a)]>scan_.range_min && scan_.ranges[aToRange(a)]<scan_.range_max) {
       if(a<interval.first)
         sumDx+=std::max(0.0,limit_*(1+abs(a))-scan_.ranges[aToRange(a)]);
       if(a>interval.second)
         sumSx+=std::max(0.0,limit_*(1+abs(a))-scan_.ranges[aToRange(a)]);
     }
  }
  
  double direction=(interval.second+interval.first)/2;
  /*if(sumDx>curve*sumSx)
    direction+=(interval.second-interval.first)/2;
  if(sumDx*curve<sumSx)
    direction-=(interval.second-interval.first)/2;
  /*if( (aToRange(M_PI/2)-aToRange(interval.second))>curve*(aToRange(interval.first)-aToRange(-M_PI/2)))
    direction-=(interval.second-interval.first)/2;
  if( (aToRange(M_PI/2)-aToRange(interval.second))*curve<(aToRange(interval.first)-aToRange(-M_PI/2)))
    direction+=(interval.second-interval.first)/2;
  
  if(minBeam>0.50)
    direction=(alphaMin>0)?direction-(interval.second-interval.first)/2:direction+(interval.second-interval.first)/2;
  if(minBeam<0.50)
    direction=(alphaMin>0)?direction-(interval.second-interval.first)/4:direction+(interval.second-interval.first)/4;
  */
  return direction;
  
}

float Controller::calcDistance()
{
  return sqrt(beams_msg.xCenter*beams_msg.xCenter+beams_msg.yCenter*beams_msg.yCenter/*+ballCenter_.z*ballCenter_.z*/);
}

bool Controller::targetInFront()
{
  if (accuFront_==3) 
    return true;
  else 
    if (abs(beams_msg.yCenter)<=centerTollerance_)
      ++accuFront_;
  return false;
}

bool Controller::isForkDown()
{
  if(scan_.ranges.size()>0)
    if(scan_.ranges[aToRange(-50*M_PI/180)]<=0.25 && scan_.ranges[aToRange(45*M_PI/180)]<=0.25)
      return true;
  return false;
}

bool Controller::isForkUp()
{
  if(scan_.ranges.size()>0)
    if(scan_.ranges[aToRange(-50*M_PI/180)]>=0.25 && scan_.ranges[aToRange(45*M_PI/180)]>=0.25)
      return true;
  return false;
}

void Controller::execute()
{
  double turn=0,lSITM=0,lGT=0;
  ObstacleAvoidanceBehav dxOA(-M_PI/2,0,12, 0.1);
  ObstacleAvoidanceBehav sxOA(0,M_PI/2,12,0.1);
  AttractorBehav goTo;
  
  
  if(scan_.ranges.size()>0)
  {
    beams_msg.beams.clear();
    for (float a = -M_PI/2; a < M_PI/2; a+=scan_.angle_increment)
    {
      if (scan_.ranges[aToRange(a)]>scan_.range_min && scan_.ranges[aToRange(a)]<scan_.range_max)
      {
        BonzoSurvivors::Beam b;
        b.degree=a;
        b.value=scan_.ranges[aToRange(a)];
        beams_msg.beams.push_back(b);
      }
    }
    if(!boost::math::isnan(ballCenter_.x))
    {
      beams_msg.xCenter=ballCenter_.x;
      beams_msg.yCenter=ballCenter_.y;
      beams_msg.zCenter=ballCenter_.z;
      if (status_==WANDERING){
        status_=FOUND;
        std::cout<<"FOUND"<<std::endl;
      }
      targetFound_=true;
    } else {
      if(status_==FOUND)
        status_=WANDERING;
    }
    if(status_==FOUND && this->calcDistance()<=nearDistance_)
    {
      status_=NEAR;
      //targetDetectionActivate_=false;
      obstacleAvoidance_=false;
      linear_=0.0f;
      std::cout<<"NEAR"<<std::endl;
    }
    if(status_==NEAR && !this->targetInFront())
    {  
      defaultTurn_=0.3f;
    }
    if(status_==NEAR && this->targetInFront())
    {
      turn=0.0f;
      if(!isForkDown())
      {
        gripper_driver::Messaggio srv;
        srv.request.cmd = ABBASSA;
        // Send command and wait for response
        if (client_.call(srv))
          ROS_INFO("Return: %ld", (long int)srv.response.ack);
        else
          ROS_ERROR("Failed to call service gripper");
        ////////////////////////////////7
        status_=FORKING;
        std::cout<<"FORKING fake"<<std::endl;
        //linear_=defaultLinear_/2;
        //////delete
      }
      else
      {
        status_=FORKING;
        std::cout<<"FORKING"<<std::endl;
        linear_=defaultLinear_/2;
      }
    }
    if(status_==FORKING)
    {
      if(scan_.ranges[aToRange(0)]>=0.2f && scan_.ranges[aToRange(M_PI/180)]>=0.2f && scan_.ranges[aToRange(-M_PI/180)]>=0.2f) 
      {
        linear_=defaultLinear_/4;
        //beams_msg.zCenter=ballCenter_.z;
      }
      else
      {
        linear_=0.0f;
        status_=LIFT;
        std::cout<<"LIFT"<<std::endl;
      }
    }
    if(status_==LIFT)
    {
      if(!isForkUp())
      {
        gripper_driver::Messaggio srv;
        srv.request.cmd = ALZA;
        // Send command and wait for response
        if (client_.call(srv))
          ROS_INFO("Return: %ld", (long int)srv.response.ack);
        else
          ROS_ERROR("Failed to call service gripper");
      }
    }
    
    if(!boost::math::isnan(ballCenter_.x) && !boost::math::isnan(ballCenter_.y))
    {
      lGT=defaultTurn_;
      lSITM=0.0;
    } 
    else
    {
      lGT=0;
      lSITM=0.4;
    }
    //Stay In The Middle
    if(lSITM>0 && status_!=NEAR && status_!=FORKING && status_!=LIFT)
    {
      //prova
      float degree1=-80.0*M_PI/180;
      float degree2=80.0*M_PI/180;
      float th=0.4f;
      float penalty=0.2f;
      bool find1=false,find2=false;
      float min=numeric_limits<float>::max();
      int minIndex=0;
      float _angleMin=-M_PI/2,_angleMax=M_PI/2,_nBeam=3;
      for(float a = _angleMin; a < _angleMax; a+=scan_.angle_increment)
      {
        if(a<=degree1 && scan_.ranges[aToRange(a)]<=th)
        find1=true;
        else if(a>=degree2 && scan_.ranges[aToRange(a)]<=th)
        find2=true;
        if(min>scan_.ranges[aToRange(a)])
        {
          min=scan_.ranges[aToRange(a)];
          minIndex=aToRange(a);
        }
      }
      for(float a = _angleMin; a < _angleMax; a+=scan_.angle_increment)
      if((a<=degree1 || a>=degree2) && (find1 || find2))
      {
        if((aToRange(a)<=scan_.ranges.size()/2 && minIndex<=scan_.ranges.size()/2))
          turn+=0.02f;
        if((aToRange(a)>scan_.ranges.size()/2 && minIndex>scan_.ranges.size()/2))
          turn-=0.02f;
      }
      //fine prova
      directionOA_=histogramOA(-M_PI/2,M_PI/2);
      turn+=2*lSITM*goTo.runToAlpha(directionOA_).angular.z;
      //std::cout<<"direction_: "<<directionOA_<<std::endl;
      turn+=lSITM*dxOA.run(scan_).angular.z*(directionOA_>0?1:1);
      turn+=lSITM*sxOA.run(scan_).angular.z*(directionOA_<0?1:1);
    }
    //Go To the attractor 
    if(lGT>0 && !this->targetInFront() && status_!=FORKING && status_!=LIFT)
      turn+=lGT*goTo.runToXY(beams_msg.xCenter,beams_msg.yCenter).angular.z;
  }
  else
  {
    beams_msg.xCenter=NAN;
    beams_msg.yCenter=NAN;
    beams_msg.zCenter=NAN;
  }
  //std::cout<<"linear_: "<<turn<<std::endl;
  cmd.linear.x = linear_*(1-min(abs(turn),1.0));
  cmd.linear.y = 0.0f;
  cmd.angular.z = fabs(turn)<0.6?turn:0.6*(turn>0?1:-1);
  
  vel_pub_.publish(cmd);
  openglPub.publish(beams_msg);
}

void Controller::newLaserScan(const sensor_msgs::LaserScan& scan_msg) {
  scan_ = scan_msg;
  if (flipped) {
    reverse(scan_.ranges.begin(),scan_.ranges.end());
  }
}

void Controller::newBallCenter(const geometry_msgs::Point& circle_msg)
{
  ballCenter_=circle_msg;
}

void Controller::newOdometry(const nav_msgs::Odometry& odom_msg) {
  odom_=odom_msg;
}

int Controller::aToRange(float a){
  return int((a-scan_.angle_min)/scan_.angle_increment);
}

int main(int argc, char** argv) {
  ros::init (argc, argv, "Controller");
  Controller c;  
  ros::Rate loop_rate(10);
  while (ros::ok()) {
    ros::spinOnce();
    c.execute();
    loop_rate.sleep();
  }
  return 0;
}
